package bme.andraskindler.treenventory.imagemanipulation;

import java.util.ArrayList;
import java.util.List;

import org.opencv.android.Utils;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point3;
import org.opencv.core.Size;
import org.opencv.utils.Converters;

import android.graphics.Bitmap;
import bme.andraskindler.treenventory.fragment.CalibrateFragment;
import bme.andraskindler.treenventory.provider.TreenventoryDataSource;

/**
 * A kalibrálást végző osztály.
 * @author andraskindler
 *
 */
public class ImageProcessor {

	private static final float SQUARE_SIDE_SIZE = 13f;

	/**
	 * Ez a függvény előállítja a kameramátrixot, a torzítási paramétereket, az eltolási vektort és a rotációs vektort. A {@link CalibrateFragment} hívja.
	 * Először megkeresi a sakktábla pontjait, majd némi Mat-átalakítás után az initCameraMatrix2D meghívásával előállítja a camera és distortion mátrixokat.
	 * Ezután solvePNP-vel az rvec és tvec előállítására is sor kerül.
	 * 
	 * @param bitmap - a fénykép, mely tartalmazza a sakktáblát.
	 */
	public static void findCorrectionMatrix(final Bitmap bitmap) {
		Mat img = new Mat();
		Utils.bitmapToMat(bitmap, img);
		final Size patternsize = new Size(7, 7);
		MatOfPoint2f corners = new MatOfPoint2f();
		System.out.println("Calib3d.findChessboardCorners corners found? : " + Calib3d.findChessboardCorners(img, patternsize, corners));

		// assigning variables to initcameramatrix2d
		final List<MatOfPoint2f> cornerList = new ArrayList<MatOfPoint2f>();
		cornerList.add(corners);
		final List<Point3> points = new ArrayList<Point3>();
		final List<MatOfPoint3f> objectpoints = new ArrayList<MatOfPoint3f>();

		for (int i = 0; i < 7; i++)
			for (int j = 0; j < 7; j++) {
				points.add(new Point3(i * SQUARE_SIDE_SIZE, j * SQUARE_SIDE_SIZE, 0));
			}
		final MatOfPoint3f tempmat = new MatOfPoint3f();
		(Converters.vector_Point3f_to_Mat(points)).convertTo(tempmat, CvType.CV_32FC2);

		objectpoints.add(tempmat);
		final Size imageSize = new Size(bitmap.getWidth(), bitmap.getHeight());
		final Mat camera = Calib3d.initCameraMatrix2D(objectpoints, cornerList, imageSize);
		System.out.println("Calib3d.initCameraMatrix2D successful");
		// camera type: CV_64F
		// distcoeffs type: CV_64F

		System.out.println("Camera matrix\n---------------------");

		for (int a = 0; a < 3; a++) {
			System.out.println();
			for (int b = 0; b < 3; b++) {
				final double[] rowitem = camera.get(a, b);
				System.out.print(rowitem[0] + " ");
			}
		}

		// calibratecamera
		List<Mat> rvecs = new ArrayList<Mat>();
		List<Mat> tvecs = new ArrayList<Mat>();
		Mat distCoeffs = new Mat();

		final List<Mat> ccObjectpoints = new ArrayList<Mat>();
		ccObjectpoints.addAll(objectpoints);

		final List<Mat> ccCornerlist = new ArrayList<Mat>();
		ccCornerlist.addAll(cornerList);

		System.out.println("\n---------------------");
		System.out.println("Calib3d.calibrateCamera: " + Calib3d.calibrateCamera(ccObjectpoints, ccCornerlist, imageSize, camera, distCoeffs, rvecs, tvecs));

		MatOfDouble convertedDistcoeffs = new MatOfDouble();
		distCoeffs.copyTo(convertedDistcoeffs);
		Mat rvec = new Mat();
		Mat tvec = new Mat();

		System.out.println(distCoeffs.type());

		final Size dist_size = distCoeffs.size();
		System.out.println("distortion matrix (" + dist_size + ")\n---------------------");
		for (int i = 0; i < dist_size.width; i++) {
			for (int j = 0; j < dist_size.height; j++) {
				final double[] rowitem = distCoeffs.get(j, i);
				System.out.print(rowitem[0] + " ");
			}
		}

		System.out.println(Calib3d.solvePnP(tempmat, corners, camera, convertedDistcoeffs, rvec, tvec));

		System.out.println("RVEC params");
		final Size rsize = rvec.size();
		for (int i = 0; i < rsize.width; i++) {
			System.out.println("---------");
			for (int j = 0; j < rsize.height; j++) {
				final double[] temp = rvec.get(j, i);
				System.out.print(temp[0] + " ");
			}
		}

		System.out.println();
		System.out.println("TVEC params");
		final Size tsize = tvec.size();
		for (int i = 0; i < tsize.width; i++) {
			System.out.println("-------");
			for (int j = 0; j < tsize.height; j++) {
				final double[] temp = tvec.get(j, i);
				System.out.println(Double.toString(temp[0]) + " ");
			}
		}

		persistExtrinsicParams(rvec, tvec, camera, distCoeffs);
	}

	/**
	 * A kamera paramétereit mentő függvény. Mivel a Mat osztály nem tárolható adatbázisban, double értékek halmazaként mentem a mátrixot.
	 * @param rvec - rotációs vektor
	 * @param tvec - eltolási vektor
	 * @param camera - kameramátrix
	 * @param distCoeffs - torzítási koefficiensek
	 */
	private static void persistExtrinsicParams(final Mat rvec, final Mat tvec, final Mat camera, final Mat distCoeffs) {
		Mat params = new Mat(new Size(4, 4), CvType.CV_32F);

		Mat rotationMatrix = new Mat();
		Calib3d.Rodrigues(tvec, rotationMatrix);
		// first row
		params.put(0, 0, rotationMatrix.get(0, 0)[0]);
		params.put(0, 1, rotationMatrix.get(0, 1)[0]);
		params.put(0, 2, rotationMatrix.get(0, 2)[0]);
		params.put(0, 3, tvec.get(0, 0)[0]);
		// second row
		params.put(1, 0, rotationMatrix.get(1, 0)[0]);
		params.put(1, 1, rotationMatrix.get(1, 1)[0]);
		params.put(1, 2, rotationMatrix.get(1, 2)[0]);
		params.put(1, 3, tvec.get(1, 0)[0]);
		// third row
		params.put(2, 0, rotationMatrix.get(2, 0)[0]);
		params.put(2, 1, rotationMatrix.get(2, 1)[0]);
		params.put(2, 2, rotationMatrix.get(2, 2)[0]);
		params.put(2, 3, tvec.get(2, 0)[0]);
		// fourth row
		params.put(3, 0, 0);
		params.put(3, 1, 0);
		params.put(3, 2, 0);
		params.put(3, 3, 1);

		params.inv();

		TreenventoryDataSource.setCameraParams(camera, distCoeffs);
	}

	/**
	 * Mátrixot olvasható formátumú stringgé alakító függvény.
	 * @param mat - a bemenő mátrix
	 * @return - olvasható szöveg
	 */
	public static String matToString(final Mat mat) {
		String result = "[";
		final Size matsize = mat.size();
		for (int i = 0; i < matsize.width; i++) {
			result += "(";
			for (int j = 0; j < matsize.height; j++) {
				result += mat.get(i, j)[0] + " ";
			}
			result += ")";
			if (i != matsize.height - 1)
				result += "\n";
		}

		return result + "]";
	}

}
